Skip to content

Commit

Permalink
[python/viewer] Add support of CoM and contact forces display. (#340)
Browse files Browse the repository at this point in the history 
* [core] Add public setter 'AbstractSensorBase.set' for sensor data.
* [core] Make sensor setter 'AbstractSensorBase.setAll' private.
* [core/python] 'result_converter' now supports Eigen::Ref. 
* [core/python] Expose setter of 'AbstractSensorBase.data'.
* [python/viewer] Fix error when using exotic matplotlib backends.
* [python/viewer] Fix 'Viewer.add_marker' and 'Panda3dApp.move_node' node placement.
* [python/viewer] Fix 'append_group' when 'remove_if_exits=False'.
* [python/viewer] Do not add legend if replay of the single log.
* [python/viewer] Hide node if scale is too small.
* [python/viewer] Add option to place cylinder anchor at bottom in Panda3d.
* [python/viewer] Add option to always display node at foreground in Panda3d, and use it for markers. 
* [python/viewer] Add method to show/hide a single node in Panda3d. 
* [python/viewer] Custom 'update_hook' can be provided to 'display' and 'replay' methods.
* [python/viewer] 'add_marker' now supports passing callables.
* [python/viewer] Make objects clickable in Panda3d.
* [python/viewer] Add helper to emulate sensor update from log, and use it in 'play_log_data' method.
* [python/viewer] Add top-level options to enable/disable display of com/contacts.

Co-authored-by: Alexis Duburcq <alexis.duburcq@wandercraft.eu>
  • Loading branch information
@duburcqa
duburcqa and Alexis Duburcq authored May 3, 2021
1 parent a7e3aa9 commit c7f3b95
Show file tree
Hide file tree
Showing 26 changed files with 939 additions and 398 deletions.
2 changes: 1 addition & 1 deletion CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
cmake_minimum_required(VERSION 3.10)

# Set the build version
set(BUILD_VERSION 1.6.14)
set(BUILD_VERSION 1.6.15)

# Set compatibility
if(CMAKE_VERSION VERSION_GREATER "3.11.0")
Expand Down
103 changes: 63 additions & 40 deletions core/include/jiminy/core/Macros.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,21 +30,8 @@ namespace jiminy
{
// **************** Generic template utilities ******************

// https://stackoverflow.com/a/34672753/4820605
template<template<typename...> class base, typename derived>
struct is_base_of_template_impl
{
template<typename... Ts>
static constexpr std::true_type test(base<Ts...> const *);
static constexpr std::false_type test(...);
using type = decltype(test(std::declval<derived *>()));
};

template<template <typename...> class base, typename derived>
using is_base_of_template = typename is_base_of_template_impl<base, derived>::type;

// https://stackoverflow.com/a/37227316/4820605
template <class F, class... Args>
template<class F, class... Args>
void do_for(F f, Args... args)
{
(f(args), ...);
Expand Down Expand Up @@ -83,6 +70,21 @@ namespace jiminy
return std::static_pointer_cast<T>(shared_from_base(derived));
}

// ======================== is_base_of_template ===========================

// https://stackoverflow.com/a/34672753/4820605
template<template<typename...> class base, typename derived>
struct is_base_of_template_impl
{
template<typename... Ts>
static constexpr std::true_type test(base<Ts...> const *);
static constexpr std::false_type test(...);
using type = decltype(test(std::declval<derived *>()));
};

template<template<typename...> class base, typename derived>
using is_base_of_template = typename is_base_of_template_impl<base, derived>::type;

// ======================== is_vector ===========================

template<typename T>
Expand Down Expand Up @@ -112,36 +114,11 @@ namespace jiminy
struct is_map : std::false_type {};

template<typename T>
struct is_map<T, typename std::enable_if<isMap<T>::value>::type> : std::true_type {};
struct is_map<T, typename std::enable_if_t<isMap<T>::value> > : std::true_type {};

template<typename T>
constexpr bool is_map_v = is_map<T>::value;

// ========================= is_eigen ===========================

namespace isEigenObjectDetail
{
template<typename T, int RowsAtCompileTime, int ColsAtCompileTime>
std::true_type test(Eigen::Matrix<T, RowsAtCompileTime, ColsAtCompileTime> const *);
template<typename T, int RowsAtCompileTime, int ColsAtCompileTime>
std::true_type test(Eigen::Ref<Eigen::Matrix<T, RowsAtCompileTime, ColsAtCompileTime> > const *);
template<typename T, int RowsAtCompileTime, int ColsAtCompileTime>
std::true_type test(Eigen::Ref<Eigen::Matrix<T, RowsAtCompileTime, ColsAtCompileTime> const> const *);
std::false_type test(...);
}

template<typename T>
struct isEigenObject : public decltype(isEigenObjectDetail::test(std::declval<std::add_pointer_t<T> >())) {};

template<typename T, typename Enable = void>
struct is_eigen : public std::false_type {};

template<typename T>
struct is_eigen<T, typename std::enable_if_t<isEigenObject<T>::value> > : std::true_type {};

template<typename T>
constexpr bool is_eigen_v = is_eigen<T>::value;

// ====================== is_not_eigen_expr =======================

// Check it is eigen object has its own storage, otherwise it is an expression
Expand Down Expand Up @@ -176,6 +153,52 @@ namespace jiminy
template<typename T>
constexpr bool is_eigen_vector_v = is_eigen_vector<T>::value;

// ====================== is_eigen_ref =======================

namespace isEigenRefDetail
{
template<typename T, int RowsAtCompileTime, int ColsAtCompileTime>
std::true_type test(Eigen::Ref<Eigen::Matrix<T, RowsAtCompileTime, ColsAtCompileTime> > const *);
template<typename T, int RowsAtCompileTime, int ColsAtCompileTime>
std::true_type test(Eigen::Ref<Eigen::Matrix<T, RowsAtCompileTime, ColsAtCompileTime> const> const *);
std::false_type test(...);
}

template<typename T>
struct isEigenRef : public decltype(isEigenRefDetail::test(std::declval<std::add_pointer_t<T> >())) {};

template<typename T, typename Enable = void>
struct is_eigen_ref : std::false_type {};

template<typename T>
struct is_eigen_ref<T, typename std::enable_if_t<isEigenRef<T>::value> > : std::true_type {};

template<typename T>
constexpr bool is_eigen_ref_v = is_eigen_ref<T>::value;

// ========================= is_eigen ===========================

namespace isEigenObjectDetail
{
template<typename T, int RowsAtCompileTime, int ColsAtCompileTime>
std::true_type test(Eigen::Matrix<T, RowsAtCompileTime, ColsAtCompileTime> const *);
template<typename T>
std::enable_if_t<is_eigen_ref_v<T>, std::true_type> test(T const *);
std::false_type test(...);
}

template<typename T>
struct isEigenObject : public decltype(isEigenObjectDetail::test(std::declval<std::add_pointer_t<T> >())) {};

template<typename T, typename Enable = void>
struct is_eigen : public std::false_type {};

template<typename T>
struct is_eigen<T, typename std::enable_if_t<isEigenObject<T>::value> > : std::true_type {};

template<typename T>
constexpr bool is_eigen_v = is_eigen<T>::value;

// =================== is_pinocchio_joint_* ===================

#define IS_PINOCCHIO_JOINT_ENABLE_IF(type, name) \
Expand Down
57 changes: 30 additions & 27 deletions core/include/jiminy/core/robot/AbstractSensor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -215,31 +215,8 @@ namespace jiminy
///////////////////////////////////////////////////////////////////////////////////////////////
configHolder_t getOptions(void) const;

///////////////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Request every sensors of the same type than the current one to record data
/// based of the input data.
///
/// \details It assumes that the internal state of the robot is consistent with the
/// input arguments.
///
/// \remark This method is not intended to be called manually. The Robot to which the
/// sensor is added is taking care of it while updating the state of the sensors.
///
/// \param[in] t Current time.
/// \param[in] q Current configuration vector of the motor.
/// \param[in] v Current velocity vector of the motor.
/// \param[in] a Current acceleration vector of the motor.
/// \param[in] uMotor Current motor effort vector of the motor.
///
/// \return Return code to determine whether the execution of the method was successful.
///
///////////////////////////////////////////////////////////////////////////////////////////////
virtual hresult_t setAll(float64_t const & t,
vectorN_t const & q,
vectorN_t const & v,
vectorN_t const & a,
vectorN_t const & uMotor) = 0;
template<typename DerivedType>
hresult_t set(Eigen::MatrixBase<DerivedType> const & value);

///////////////////////////////////////////////////////////////////////////////////////////////
///
Expand Down Expand Up @@ -331,6 +308,32 @@ namespace jiminy
virtual std::vector<std::string> const & getFieldnames(void) const = 0;

protected:
///////////////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Request every sensors of the same type than the current one to record data
/// based of the input data.
///
/// \details It assumes that the internal state of the robot is consistent with the
/// input arguments.
///
/// \remark This method is not intended to be called manually. The Robot to which the
/// sensor is added is taking care of it while updating the state of the sensors.
///
/// \param[in] t Current time.
/// \param[in] q Current configuration vector of the motor.
/// \param[in] v Current velocity vector of the motor.
/// \param[in] a Current acceleration vector of the motor.
/// \param[in] uMotor Current motor effort vector of the motor.
///
/// \return Return code to determine whether the execution of the method was successful.
///
///////////////////////////////////////////////////////////////////////////////////////////////
virtual hresult_t setAll(float64_t const & t,
vectorN_t const & q,
vectorN_t const & v,
vectorN_t const & a,
vectorN_t const & uMotor) = 0;

///////////////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Request the sensor to record data based of the input data.
Expand Down Expand Up @@ -453,13 +456,13 @@ namespace jiminy
virtual uint64_t getSize(void) const override final;

virtual Eigen::Ref<vectorN_t const> get(void) const override final;

protected:
virtual hresult_t setAll(float64_t const & t,
vectorN_t const & q,
vectorN_t const & v,
vectorN_t const & a,
vectorN_t const & uMotor) override final;

protected:
virtual Eigen::Ref<vectorN_t> get(void) override final;
virtual Eigen::Ref<vectorN_t> data(void) override final;

Expand Down
Loading

0 comments on commit c7f3b95

Please sign in to comment.