Skip to content

Commit

Permalink
Update implementation.
Browse files Browse the repository at this point in the history
  • Loading branch information
@mmurooka
mmurooka committed Dec 30, 2024
1 parent df417e6 commit d4fc220
Show file tree
Hide file tree
Showing 8 changed files with 419 additions and 309 deletions.
93 changes: 65 additions & 28 deletions etc/HumanRetargetingController.in.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -94,6 +94,28 @@ FootTaskList:
stiffness: 1000.0
weight: 1000.0

RetargetingTaskList:
LeftElbow:
type: transform
frame: LeftElbowFrame
weight: 500.0
dimWeight: [0.0, 0.0, 0.0, 1.0, 1.0, 1.0]
LeftWrist:
type: transform
frame: LeftWristFrame
weight: 500.0
dimWeight: [0.0, 0.0, 0.0, 1.0, 1.0, 1.0]
RightElbow:
type: transform
frame: RightElbowFrame
weight: 500.0
dimWeight: [0.0, 0.0, 0.0, 1.0, 1.0, 1.0]
RightWrist:
type: transform
frame: RightWristFrame
weight: 500.0
dimWeight: [0.0, 0.0, 0.0, 1.0, 1.0, 1.0]

FootManager:
name: FootManager
footstepDuration: 0.8 # [sec]
Expand Down Expand Up @@ -199,24 +221,26 @@ CentroidalManager:
RetargetingManagerSet:
name: RetargetingManagerSet
humanWaistPoseTopicName: /hrc/poses/waist
humanWaistPoseFromOrigin:
translation: [1.0, 0.0, 0.8]
pointMarkerSize: 0.15
phaseMarkerPoseOffset:
translation: [0.0, 0.0, 1.0]
ArmRetargetingManagerList:
Left:
humanElbowPoseTopicName: /hrc/poses/left_elbow
humanWristPoseTopicName: /hrc/poses/left_wrist
elbowBodyPart:: LeftElbow
wristBodyPart: LeftWrist
elbowTaskName: LeftElbow
wristTaskName: LeftWrist
stiffness: 10.0
Right:
humanElbowPoseTopicName: /hrc/poses/right_elbow
humanWristPoseTopicName: /hrc/poses/right_wrist
elbowBodyPart:: RightElbow
wristBodyPart: RightWrist
elbowTaskName: RightElbow
wristTaskName: RightWrist
stiffness: 10.0

# OverwriteConfigKeys: [NoSensors]
OverwriteConfigKeys: [NoSensors]

OverwriteConfigList:
NoSensors:
Expand Down Expand Up @@ -288,38 +312,22 @@ robots:
frame: WAIST_R_S

RetargetingTaskList:
- bodyPart: LeftElbow
type: transform
frame: LeftElbowFrame
weight: 500.0
dimWeight: [0.0, 0.0, 0.0, 1.0, 1.0, 1.0]
LeftElbow:
activeJoints: [
"Root",
"WAIST_Y", # "WAIST_P", "WAIST_R",
"L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y"]
- bodyPart: LeftWrist
type: transform
frame: LeftWristFrame
weight: 500.0
dimWeight: [0.0, 0.0, 0.0, 1.0, 1.0, 1.0]
LeftWrist:
activeJoints: [
"Root",
"WAIST_Y", # "WAIST_P", "WAIST_R",
"L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y", "L_ELBOW_P"]
- bodyPart: RightElbow
type: transform
frame: RightElbowFrame
weight: 500.0
dimWeight: [0.0, 0.0, 0.0, 1.0, 1.0, 1.0]
RightElbow:
activeJoints: [
"Root",
"WAIST_Y", # "WAIST_P", "WAIST_R",
"R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y"]
- bodyPart: RightWrist
type: transform
frame: RightWristFrame
weight: 500.0
dimWeight: [0.0, 0.0, 0.0, 1.0, 1.0, 1.0]
RightWrist:
activeJoints: [
"Root",
"WAIST_Y", # "WAIST_P", "WAIST_R",
Expand All @@ -329,7 +337,8 @@ robots:
robotBaseLinkName: WAIST_R_S
syncJoints: [
"WAIST_Y", # "WAIST_P", "WAIST_R",
"L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y", "L_ELBOW_P"]
"L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y", "L_ELBOW_P",
"R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y", "R_ELBOW_P"]
ArmRetargetingManagerList:
Left:
robotCalibPostures:
Expand Down Expand Up @@ -357,7 +366,21 @@ robots:
L_ELBOW_Y: 0.0
L_WRIST_R: 0.0
L_WRIST_Y: 0.0
# calibResultConfig: {}
calibResultConfig:
humanTransFromBaseToShoulder:
translation: [0.0, 0.3, 0.3]
rotation: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
robotTransFromBaseToShoulder:
translation: [0.03, 0.24, 0.28]
rotation: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
elbowRotTransFromHumanToRobot:
translation: [0.0, 0.0, 0.0]
rotation: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
wristRotTransFromHumanToRobot:
translation: [0.0, 0.0, 0.0]
rotation: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
elbowScale: 0.8
wristScale: 0.8
Right:
robotCalibPostures:
X:
Expand All @@ -384,7 +407,21 @@ robots:
R_ELBOW_Y: 0.0
R_WRIST_R: 0.0
R_WRIST_Y: 0.0
# calibResultConfig: {}
calibResultConfig:
humanTransFromBaseToShoulder:
translation: [0.0, -0.3, 0.3]
rotation: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
robotTransFromBaseToShoulder:
translation: [0.03, -0.24, 0.28]
rotation: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
elbowRotTransFromHumanToRobot:
translation: [0.0, 0.0, 0.0]
rotation: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
wristRotTransFromHumanToRobot:
translation: [0.0, 0.0, 0.0]
rotation: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
elbowScale: 0.8
wristScale: 0.8

frames:
- name: LeftElbowFrame
Expand Down
39 changes: 21 additions & 18 deletions include/HumanRetargetingController/ArmRetargetingManager.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,11 +41,11 @@ class ArmRetargetingManager
//! Topic name of human wrist pose
std::string humanWristPoseTopicName;

//! Body part of elbow retargeting task
std::string elbowBodyPart;
//! Name of elbow retargeting task
std::string elbowTaskName;

//! Body part of wrist retargeting task
std::string wristBodyPart;
//! Name of wrist retargeting task
std::string wristTaskName;

//! Retargeting task stiffness
Eigen::Vector6d stiffness = Eigen::Vector6d::Constant(10);
Expand Down Expand Up @@ -103,8 +103,8 @@ class ArmRetargetingManager
*/
void load(const mc_rtc::Configuration & mcRtcConfig);

/** \brief Dump in YAML format. */
void dump() const;
/** \brief Get YAML format string. */
std::string dump() const;
};

/** \brief Calibration source.
Expand Down Expand Up @@ -180,6 +180,12 @@ class ArmRetargetingManager
/** \brief Accessor to the ROS pose manager for human waist pose. */
const std::shared_ptr<RosPoseManager> & humanWaistPoseManager() const;

/** \brief Accessor to the retargeting task of elbow. */
const std::shared_ptr<mc_tasks::TransformTask> & elbowTask() const;

/** \brief Accessor to the retargeting task of wrist. */
const std::shared_ptr<mc_tasks::TransformTask> & wristTask() const;

/** \brief Update target of retargeting task. */
void updateTaskTarget(const std::shared_ptr<mc_tasks::TransformTask> & task, const sva::PTransformd & pose);

Expand All @@ -189,9 +195,6 @@ class ArmRetargetingManager
/** \brief Set robot data as calibration source. */
void setRobotCalibSource(const std::string & axis);

/** \brief Clear robot for calibration. */
void clearCalibRobot();

/** \brief Update calibration. */
void updateCalib();

Expand All @@ -215,6 +218,15 @@ class ArmRetargetingManager
//! ROS pose manager for human wrist pose
std::shared_ptr<RosPoseManager> humanWristPoseManager_;

//! Measured pose of human shoulder (expressed relative to human origin)
std::optional<sva::PTransformd> humanShoulderPose_ = std::nullopt;

//! Measured pose of human elbow (expressed relative to human origin)
std::optional<sva::PTransformd> humanElbowPose_ = std::nullopt;

//! Measured pose of human wrist (expressed relative to human origin)
std::optional<sva::PTransformd> humanWristPose_ = std::nullopt;

//! Target pose of robot shoulder
std::optional<sva::PTransformd> robotShoulderPose_ = std::nullopt;

Expand All @@ -224,12 +236,6 @@ class ArmRetargetingManager
//! Target pose of robot wrist
std::optional<sva::PTransformd> robotWristPose_ = std::nullopt;

//! Retargeting task of elbow
std::shared_ptr<mc_tasks::TransformTask> elbowTask_;

//! Retargeting task of wrist
std::shared_ptr<mc_tasks::TransformTask> wristTask_;

//! Function to interpolate task stiffness
std::shared_ptr<TrajColl::CubicInterpolator<double>> stiffnessRatioFunc_;

Expand All @@ -241,8 +247,5 @@ class ArmRetargetingManager

//! Calibration result
CalibResult calibResult_;

//! Robot for calibration
std::shared_ptr<mc_rbdyn::Robots> calibRobots_;
};
} // namespace HRC
12 changes: 12 additions & 0 deletions include/HumanRetargetingController/RetargetingManagerSet.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,9 @@ class RetargetingManagerSet : public std::unordered_map<ArmSide, std::shared_ptr
//! Joints that update the target position at the end of retargeting
std::vector<std::string> syncJoints;

//! Human waist pose from origin
sva::PTransformd humanWaistPoseFromOrigin = sva::PTransformd::Identity();

//! Point marker size
double pointMarkerSize = 0.15;

Expand Down Expand Up @@ -118,13 +121,22 @@ class RetargetingManagerSet : public std::unordered_map<ArmSide, std::shared_ptr
/** \brief Update GUI. */
void updateGUI();

/** \brief Make robot for calibration. */
void makeCalibRobot();

/** \brief Clear robot for calibration. */
void clearCalibRobot();

public:
//! Whether it is ready for retargeting or not
bool isReady_ = false;

//! Whether retargeting is enabled or not
bool isEnabled_ = false;

//! Robot for calibration
std::shared_ptr<mc_rbdyn::Robots> calibRobots_;

//! ROS node handle
std::shared_ptr<ros::NodeHandle> nh_;

Expand Down
Loading

0 comments on commit d4fc220

Please sign in to comment.