def get_constraint(self):
"""
example:
name='JointPosition'
parameter_value_pair='{
"joint_name": "torso_lift_joint", #required
"goal_position": 0, #required
"weight": 1, #optional
"gain": 10, #optional -- error is multiplied with this value
"max_speed": 1 #optional -- rad/s or m/s depending on joint; can not go higher than urdf limit
}'
:return:
"""
current_joint = self.get_input_joint_position(self.joint_name)
joint_goal = self.get_input_float(self.goal)
weight = self.get_input_float(self.weight)
# p_gain = self.get_input_float(self.gain)
max_speed = self.get_input_float(self.max_speed)
t = self.get_input_sampling_period()
soft_constraints = OrderedDict()
if self.get_robot().is_joint_continuous(self.joint_name):
err = w.shortest_angular_distance(current_joint, joint_goal)
else:
err = joint_goal - current_joint
capped_err = w.diffable_max_fast(w.diffable_min_fast(err, max_speed * t), -max_speed * t)
soft_constraints[str(self)] = SoftConstraint(lower=capped_err,
upper=capped_err,
weight=weight,
expression=current_joint)
return soft_constraints
def get_constraint(self):
"""
example:
name='CartesianPosition'
parameter_value_pair='{
"root": "base_footprint", #required
"tip": "r_gripper_tool_frame", #required
"goal_position": {"header":
{"stamp":
{"secs": 0,
"nsecs": 0},
"frame_id": "",
"seq": 0},
"pose": {"position":
{"y": 0.0,
"x": 0.0,
"z": 0.0},
"orientation": {"y": 0.0,
"x": 0.0,
"z": 0.0,
"w": 0.0}
}
}', #required
"weight": 1, #optional
"gain": 3, #optional -- error is multiplied with this value
"max_speed": 0.3 #optional -- rad/s or m/s depending on joint; can not go higher than urdf limit
}'
:return:
"""
goal_position = w.position_of(self.get_goal_pose())
weight = self.get_input_float(self.weight)
gain = self.get_input_float(self.gain)
max_speed = self.get_input_float(self.max_speed)
t = self.get_input_sampling_period()
current_position = w.position_of(self.get_fk(self.root, self.tip))
soft_constraints = OrderedDict()
trans_error_vector = goal_position - current_position
trans_error = w.norm(trans_error_vector)
trans_scale = w.diffable_min_fast(trans_error * gain, max_speed * t)
trans_control = w.save_division(trans_error_vector, trans_error) * trans_scale
soft_constraints[str(self) + u'x'] = SoftConstraint(lower=trans_control[0],
upper=trans_control[0],
weight=weight,
expression=current_position[0])
soft_constraints[str(self) + u'y'] = SoftConstraint(lower=trans_control[1],
upper=trans_control[1],
weight=weight,
expression=current_position[1])
soft_constraints[str(self) + u'z'] = SoftConstraint(lower=trans_control[2],
upper=trans_control[2],
weight=weight,
expression=current_position[2])
return soft_constraints
def get_constraint(self):
goal_position = w.position_of(self.get_goal_pose())
weight = self.get_input_float(self.weight)
gain = self.get_input_float(self.gain)
max_speed = self.get_input_float(self.max_speed)
t = self.get_input_sampling_period()
current_position = w.position_of(self.get_fk(self.root, self.tip))
soft_constraints = OrderedDict()
trans_error_vector = goal_position - current_position
trans_error = w.norm(trans_error_vector)
trans_scale = w.diffable_min_fast(trans_error * gain, max_speed * t)
trans_control = w.save_division(trans_error_vector, trans_error) * trans_scale
soft_constraints[str(self) + u'y'] = SoftConstraint(lower=trans_control[1],
upper=trans_control[1],
weight=weight,
expression=current_position[1])
return soft_constraints
def get_constraint(self):
"""
example:
name='CartesianPosition'
parameter_value_pair='{
"root": "base_footprint", #required
"tip": "r_gripper_tool_frame", #required
"goal_position": {"header":
{"stamp":
{"secs": 0,
"nsecs": 0},
"frame_id": "",
"seq": 0},
"pose": {"position":
{"y": 0.0,
"x": 0.0,
"z": 0.0},
"orientation": {"y": 0.0,
"x": 0.0,
"z": 0.0,
"w": 0.0}
}
}', #required
"weight": 1, #optional
"gain": 3, #optional -- error is multiplied with this value
"max_speed": 0.3 #optional -- rad/s or m/s depending on joint; can not go higher than urdf limit
}'
:return:
"""
goal_rotation = w.rotation_of(self.get_goal_pose())
weight = self.get_input_float(self.weight)
gain = self.get_input_float(self.gain)
max_speed = self.get_input_float(self.max_speed)
t = self.get_input_sampling_period()
current_rotation = w.rotation_of(self.get_fk(self.root, self.tip))
current_evaluated_rotation = w.rotation_of(self.get_fk_evaluated(self.root, self.tip))
soft_constraints = OrderedDict()
angle = w.rotation_distance(current_rotation, goal_rotation)
angle = w.diffable_abs(angle)
capped_angle = w.diffable_min_fast(w.save_division(max_speed * t, (gain * angle)), 1)
q1 = w.quaternion_from_matrix(current_rotation)
q2 = w.quaternion_from_matrix(goal_rotation)
intermediate_goal = w.diffable_slerp(q1, q2, capped_angle)
tmp_q = w.quaternion_diff(q1, intermediate_goal)
axis3, angle3 = w.axis_angle_from_quaternion(tmp_q[0], tmp_q[1], tmp_q[2], tmp_q[3])
r_rot_control = axis3 * angle3
hack = w.rotation_matrix_from_axis_angle([0, 0, 1], 0.0001)
axis2, angle2 = w.diffable_axis_angle_from_matrix(
w.dot(current_rotation.T, w.dot(current_evaluated_rotation, hack)).T)
c_aa = (axis2 * angle2)
soft_constraints[str(self) + u'/0'] = SoftConstraint(lower=r_rot_control[0],
upper=r_rot_control[0],
weight=weight,
expression=c_aa[0])
soft_constraints[str(self) + u'/1'] = SoftConstraint(lower=r_rot_control[1],
upper=r_rot_control[1],
weight=weight,
expression=c_aa[1])
soft_constraints[str(self) + u'/2'] = SoftConstraint(lower=r_rot_control[2],
upper=r_rot_control[2],
weight=weight,
expression=c_aa[2])
return soft_constraints
def get_constraint(self):
"""
example:
name='CartesianPosition'
parameter_value_pair='{
"root": "base_footprint", #required
"tip": "r_gripper_tool_frame", #required
"goal_position": {"header":
{"stamp":
{"secs": 0,
"nsecs": 0},
"frame_id": "",
"seq": 0},
"pose": {"position":
{"y": 0.0,
"x": 0.0,
"z": 0.0},
"orientation": {"y": 0.0,
"x": 0.0,
"z": 0.0,
"w": 0.0}
}
}', #required
"weight": 1, #optional
"gain": 3, #optional -- error is multiplied with this value
"max_speed": 0.3 #optional -- rad/s or m/s depending on joint; can not go higher than urdf limit
}'
:return:
"""
goal_rotation = w.rotation_of(self.get_goal_pose())
weight = self.get_input_float(self.weight)
gain = self.get_input_float(self.gain)
max_speed = self.get_input_float(self.max_speed)
current_rotation = w.rotation_of(self.get_fk(self.root, self.tip))
current_evaluated_rotation = w.rotation_of(self.get_fk_evaluated(self.root, self.tip))
soft_constraints = OrderedDict()
axis, angle = w.diffable_axis_angle_from_matrix(w.dot(current_rotation.T, goal_rotation))
capped_angle = w.diffable_max_fast(w.diffable_min_fast(gain * angle, max_speed), -max_speed)
r_rot_control = axis * capped_angle
hack = w.rotation_matrix_from_axis_angle([0, 0, 1], 0.0001)
axis, angle = w.diffable_axis_angle_from_matrix(
w.dot(current_rotation.T, w.dot(current_evaluated_rotation, hack)).T)
c_aa = (axis * angle)
soft_constraints[str(self) + u'/0'] = SoftConstraint(lower=r_rot_control[0],
upper=r_rot_control[0],
weight=weight,
expression=c_aa[0])
soft_constraints[str(self) + u'/1'] = SoftConstraint(lower=r_rot_control[1],
upper=r_rot_control[1],
weight=weight,
expression=c_aa[1])
soft_constraints[str(self) + u'/2'] = SoftConstraint(lower=r_rot_control[2],
upper=r_rot_control[2],
weight=weight,
expression=c_aa[2])
return soft_constraints