def get_angle_casadi(self, point1, point2, point3):
v12 = w.vector3(*point1) - w.vector3(*point2)
v13 = w.vector3(*point1) - w.vector3(*point3)
v23 = w.vector3(*point2) - w.vector3(*point3)
d12 = w.norm(v12)
d13 = w.norm(v13)
d23 = w.norm(v23)
# return w.acos(w.dot(v12, v13.T)[0] / (d12 * d13))
return w.acos((d12**2 + d13**2 - d23**2) / (2 * d12 * d13))
def get_constraint(self):
soft_constraints = OrderedDict()
weight = self.get_input_float(self.weight)
root_T_tip = self.get_fk(self.root, self.tip)
goal_point = self.get_goal_point()
pointing_axis = self.get_pointing_axis()
goal_axis = goal_point - w.position_of(root_T_tip)
goal_axis /= w.norm(goal_axis) # FIXME possible /0
current_axis = w.dot(root_T_tip, pointing_axis)
diff = goal_axis - current_axis
soft_constraints[str(self) + u'x'] = SoftConstraint(lower=diff[0],
upper=diff[0],
weight=weight,
expression=current_axis[0])
soft_constraints[str(self) + u'y'] = SoftConstraint(lower=diff[1],
upper=diff[1],
weight=weight,
expression=current_axis[1])
soft_constraints[str(self) + u'z'] = SoftConstraint(lower=diff[2],
upper=diff[2],
weight=weight,
expression=current_axis[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_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